

#include "rviz_custom_plugins/panels/snapshot_point_panel.h"
#include "rviz_custom_plugins/utils/interactive_marker_manager.h"
#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_factory.h"
#include "rviz_custom_plugins/utils/project_manager.h"
#include "rviz_custom_plugins/models/point_normals.h"
#include <QDir>
#include <QFile>
#include <QTextStream>
#include <nlohmann/json.hpp>


using rviz_custom_plugins::utils::addMetadata;
using rviz_custom_plugins::utils::InteractiveMarkerManager;
using rviz_custom_plugins::utils::getMetadataItem;
using rviz_custom_plugins::utils::InteractiveMarkerFactory;

using rviz_custom_plugins::utils::MARKER_ADDED;
using rviz_custom_plugins::utils::MARKER_UPDATED;
using rviz_custom_plugins::utils::MARKER_REMOVED;
using rviz_custom_plugins::models::PointNormals;
using rviz_custom_plugins::models::PointNormal;

using rviz_custom_plugins::utils::ProjectManager;


namespace rviz_custom_plugins
{

namespace panels
{


SnapshotPointPanel::SnapshotPointPanel(QWidget *parent) : rviz_common::Panel(parent), ui(new Ui::snapshotPointPanel())
{
    ui->setupUi(this);
    ProjectManager* projectManager = ProjectManager::getInstance();
    tableModel = new PointModel();
    ui->tableSnapshotPoints->setModel(tableModel);
    ui->tableSnapshotPoints->setSelectionMode(QAbstractItemView::SingleSelection);
    ui->tableSnapshotPoints->setSelectionBehavior(QAbstractItemView::SelectRows);
    // 设置列自动拉伸
    ui->tableSnapshotPoints->horizontalHeader()->setSectionResizeMode(0, QHeaderView::Stretch);
    ui->tableSnapshotPoints->horizontalHeader()->setSectionResizeMode(1, QHeaderView::Stretch);
    ui->tableSnapshotPoints->horizontalHeader()->setSectionResizeMode(2, QHeaderView::Stretch);
    ui->tableSnapshotPoints->show();

    ui->tableSnapshotPoints->setContextMenuPolicy(Qt::CustomContextMenu);

    mapNotifier = BagMapNotifier::getInstance();

    qRegisterMetaType<interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr>(
        "interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr"
    );

    connect(tableModel, &PointModel::itemOrderUpdated, this, &SnapshotPointPanel::handlerMarkersReorder);
    connect(ui->btnRemove, &QPushButton::clicked, this, &SnapshotPointPanel::removeButtonClicked);

    connect(mapNotifier, &BagMapNotifier::mapPointSelected, this, &SnapshotPointPanel::handleMapPointSelected);

    connect(projectManager, &ProjectManager::projectCreated, this, &SnapshotPointPanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectOpened, this, &SnapshotPointPanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectSaved, this, &SnapshotPointPanel::handleProjectSaved, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectClosed, this, &SnapshotPointPanel::handleProjectClosed, Qt::DirectConnection);

}


void SnapshotPointPanel::load(const Config& config)
{
    Panel::load(config);
    // set marker server namespace
    QString nameSpace;
    config.mapGetString("NameSpace", &nameSpace);
    markerThread = InteractiveMarkerManager::getInstance()->addThread(nameSpace);
    connect(markerThread.get(), &InteractiveMarkerThread::interactiveMarkerFeedback, this, &SnapshotPointPanel::handleMarkerFeedback, Qt::QueuedConnection);
    connect(markerThread.get(), &InteractiveMarkerThread::activeStatusChanged, this, &SnapshotPointPanel::handleActiveStatusChanged, Qt::QueuedConnection);
    handleActiveStatusChanged(markerThread->getActive());
}


void SnapshotPointPanel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback) {

    // std::ostringstream sstm;
    // sstm << " feedback->marker_name:" << feedback->marker_name;
    // RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

    // 处理新增marker
    if(MARKER_ADDED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "add marker marker_name:" << feedback->marker_name;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
        return;
    }
    else if(MARKER_UPDATED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "update marker marker_name:" << feedback->marker_name;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
        return;
    }
    else if(MARKER_REMOVED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "remove marker marker_name:" << feedback->marker_name;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

        return;
    }


    // switch (feedback->event_type) {
    //     case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE:
    //     {
    //         QString markerName = QString::fromStdString(feedback->marker_name);
    //         int row = tableModel->getMarkerRow(markerName);
    //         QModelIndex index = tableModel->index(row, 0); // 获取行首单元格索引
    //         ui->tableSnapshotPoints->setCurrentIndex(index);
    //         tableModel->handleMarkerFeedback(feedback);
    //         break;
    //     }
    // }
}


void SnapshotPointPanel::handleActiveStatusChanged(bool active) {
    if (active) {
        setEnabled(true);
        parentWidget()->show();
    } else {
        setEnabled(false);
        parentWidget()->hide();
    }
}


void SnapshotPointPanel::removeButtonClicked()
{
    QModelIndex currentIndex = ui->tableSnapshotPoints->selectionModel()->currentIndex();
    if(currentIndex.isValid()) {
        int currentRow = currentIndex.row(); // 当前行号
        removeItem(currentRow);
    }
}


void SnapshotPointPanel::removeItem(int row)
{
    InteractiveMarker intMarker = tableModel->getRowItem(row);
    markerThread->erase(intMarker.name);

    // QString markerName = QString::fromStdString(intMarker.name);
    // int markerRow = tableModel->getItemRow(markerName);
    tableModel->removeItem(row);
}


void SnapshotPointPanel::handlerMarkersReorder(int row, std::list<InteractiveMarker>& markers) {
    auto it = markers.begin();
    std::advance(it, row); // 移动到指定索引

    for ( ;it != markers.end();++it) {
        int markerShowRow = getMetadataItem(*it, ROW, "-1").toInt() + 1;
        it->description = std::to_string(markerShowRow);
        markerThread->insert(*it);
    }
}


void SnapshotPointPanel::handleMapPointSelected(QString nameSpace, int pclIndex, pcl::PointXYZINormal& pclPoint)
{
    if(nameSpace == markerThread->objectName()) 
    {
        auto pointNormal = PointNormal::fromPclPoint(pclPoint);
        InteractiveMarker intMarker = InteractiveMarkerFactory::createPcdPoint(pointNormal);
        markerThread->insert(intMarker);
        tableModel->insertItem(tableModel->rowCount(), intMarker);
    }
}



void SnapshotPointPanel::handleProjectLoaded(QSharedPointer<QVariantMap> projectData)
{
    
    QVariant pointsVar = (*projectData)["photoPoints"];
    PointNormals points = pointsVar.value<PointNormals>();

    for (const auto& point : points.points)
    {
        InteractiveMarker intMarker = InteractiveMarkerFactory::createPcdPoint(point);
        // addMetadata(intMarker, "pcl_index", QString::number(pose.index));

        markerThread->insert(intMarker);
        tableModel->insertItem(point.id - 1, intMarker);
    }
    
}


void SnapshotPointPanel::handleProjectSaved(QSharedPointer<QVariantMap> projectData)
{
    auto markers = tableModel->getMarkers();
    // std::ostringstream sstm;
    // sstm << " markers size:" << markers.size();
    // RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

    PointNormals points;

    int index = 1;
    for (auto it = markers.begin(); it != markers.end(); it++, ++index) 
    {
        QString pointStr = getMetadataItem(*it, "point_normal");
        PointNormal point = json::parse(pointStr.toStdString()).get<PointNormal>();
        point.id = index;
        points.points.push_back(point);
    }

    (*projectData)["photoPoints"] = QVariant::fromValue(points);
}


void SnapshotPointPanel::handleProjectClosed(QSharedPointer<QVariantMap> projectData)
{
    int lastRow = tableModel->rowCount() - 1;
    for(;lastRow >= 0; lastRow--)
    {
        removeItem(lastRow);
    }
} 


}

}



#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::panels::SnapshotPointPanel, rviz_common::Panel )
